/**
 * Copyright (c) 2009 blurredVision
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and
 * associated documentation files (the "Software"), to deal in the Software without restriction, 
 * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or 
 * substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT 
 * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT 
 * OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
#include "StdAfx.h"
#include "Matrix.h"
#include "Vector.h"
#include <iostream>

//by dennis
#include "Quaternion.h"

#define _m(i,j) m[(i)*4+(j)]

// Uses the matrix flipped
#define _mf(j,i) m[(i)*4+(j)] 

namespace math {

Matrix4::Matrix4()
{
	setIdentity();
}

Matrix4::Matrix4(float*val)
{
	for (int i=0; i<16; ++i) 
		m[i] = val[i];
}

Matrix4::Matrix4(float m0, float m1, float m2, float m3, 
				float m4, float m5, float m6, float m7, 
				float m8, float m9, float m10, float m11, 
				float m12, float m13, float m14, float m15)
{
	m[0] = m0; m[1] = m1; m[2] = m2; m[3] = m3;
	m[4] = m4; m[5] = m5; m[6] = m6; m[7] = m7;
	m[8] = m8; m[9] = m9; m[10] = m10; m[11] = m11;
	m[12] = m12; m[13] = m13; m[14] = m14; m[15] = m15;
}

Matrix4::Matrix4(const Matrix4 &mat)
{
	for (int i=0; i<16; ++i) 
		m[i] = mat.m[i];
}


Matrix4::~Matrix4()
{

}

Matrix4& Matrix4::operator=(const Matrix4 &src)
{
	m[0]=src.m[0];   m[1]=src.m[1];   m[2]=src.m[2];   m[3]=src.m[3];
	m[4]=src.m[4];   m[5]=src.m[5];   m[6]=src.m[6];   m[7]=src.m[7];
	m[8]=src.m[8];   m[9]=src.m[9];   m[10]=src.m[10]; m[11]=src.m[11];
	m[12]=src.m[12]; m[13]=src.m[13]; m[14]=src.m[14]; m[15]=src.m[15];

	return (*this);
}

Matrix4 Matrix4::transpose(void) const
{
	Matrix4 ret;

	ret.m[0]=m[0]; ret.m[1]=m[4]; ret.m[2]=m[8]; ret.m[3]=m[12];
	ret.m[4]=m[1]; ret.m[5]=m[5]; ret.m[6]=m[9]; ret.m[7]=m[13];
	ret.m[8]=m[2]; ret.m[9]=m[6]; ret.m[10]=m[10]; ret.m[11]=m[14];
	ret.m[12]=m[3]; ret.m[13]=m[7]; ret.m[14]=m[11]; ret.m[15]=m[15];

	return ret;
}

void Matrix4::inverseTranspose()
{
	if (!inv()) 
		return;

	transpose();
}


inline Matrix4 Matrix4::operator+(const Matrix4 &src) const
{
	Matrix4 ret;

	ret.m[0]+=src.m[0]; ret.m[1]+=src.m[1]; ret.m[2]+=src.m[2]; ret.m[3]+=src.m[3];
	ret.m[4]+=src.m[4]; ret.m[5]+=src.m[5]; ret.m[6]+=src.m[6]; ret.m[7]+=src.m[7];
	ret.m[8]+=src.m[8]; ret.m[9]+=src.m[9]; ret.m[10]+=src.m[10]; ret.m[11]+=src.m[11];
	ret.m[12]+=src.m[12]; ret.m[13]+=src.m[13]; ret.m[14]+=src.m[14]; ret.m[15]+=src.m[15];

	return ret;
}

inline  Matrix4 Matrix4::operator-(const Matrix4 &src) const
{
	Matrix4 ret;

	ret.m[0]-=src.m[0]; ret.m[1]-=src.m[1]; ret.m[2]-=src.m[2]; ret.m[3]-=src.m[3];
	ret.m[4]-=src.m[4]; ret.m[5]-=src.m[5]; ret.m[6]-=src.m[6]; ret.m[7]-=src.m[7];
	ret.m[8]-=src.m[8]; ret.m[9]-=src.m[9]; ret.m[10]-=src.m[10]; ret.m[11]-=src.m[11];
	ret.m[12]-=src.m[12]; ret.m[13]-=src.m[13]; ret.m[14]-=src.m[14]; ret.m[15]-=src.m[15];

	return ret;
}

inline void Matrix4::operator+=(const Matrix4 &src)
{
	(*this) = (*this) + src;
}

inline void Matrix4::operator-=(const Matrix4 &src)
{
	(*this) = (*this) - src;
}

Vector3 Matrix4::operator*(const Vector3 &vec) const
{
	return Vector3( vec.x * m[0] + vec.y * m[4] + vec.z * m[8] + m[12],
					vec.x * m[1] + vec.y * m[5] + vec.z * m[9] + m[13],
					vec.x * m[2] + vec.y * m[6] + vec.z * m[10] + m[14] );
}

Vector4 Matrix4::operator*(const Vector4 &vec) const
{
	return Vector4( vec.x * m[0] + vec.y * m[1] + vec.z * m[2] + vec.w * m[3],
					vec.x * m[4] + vec.y * m[5] + vec.z * m[6] + vec.w * m[7],
					vec.x * m[8] + vec.y * m[9] + vec.z * m[10] + vec.w * m[11],
					vec.x * m[12] + vec.y * m[13] + vec.z * m[14] + vec.w * m[15]	);
}

Matrix4 Matrix4::operator*(const Matrix4 &src) const
{
	Matrix4 ret;

	ret.m[0] = m[0] * src.m[0] + m[1] * src.m[4] + m[2] * src.m[8] + m[3] * src.m[12];
	ret.m[1] = m[0] * src.m[1] + m[1] * src.m[5] + m[2] * src.m[9] + m[3] * src.m[13];
	ret.m[2] = m[0] * src.m[2] + m[1] * src.m[6] + m[2]* src.m[10] + m[3] * src.m[14];
	ret.m[3] = m[0] * src.m[3] + m[1] * src.m[7] + m[2]* src.m[11] + m[3] * src.m[15];

	ret.m[4] = m[4] * src.m[0] + m[5] * src.m[4] + m[6] * src.m[8] + m[7] * src.m[12];
	ret.m[5] = m[4] * src.m[1] + m[5] * src.m[5] + m[6] * src.m[9] + m[7] * src.m[13];
	ret.m[6] = m[4] * src.m[2] + m[5] * src.m[6] + m[6]* src.m[10] + m[7] * src.m[14];
	ret.m[7] = m[4] * src.m[3] + m[5] * src.m[7] + m[6]* src.m[11] + m[7] * src.m[15];

	ret.m[8] = m[8] * src.m[0] + m[9] * src.m[4] + m[10] * src.m[8] + m[11] * src.m[12];
	ret.m[9] = m[8] * src.m[1] + m[9] * src.m[5] + m[10] * src.m[9] + m[11] * src.m[13];
	ret.m[10]= m[8] * src.m[2] + m[9] * src.m[6] + m[10] * src.m[10] + m[11] * src.m[14];
	ret.m[11]= m[8] * src.m[3] + m[9] * src.m[7] + m[10] * src.m[11] + m[11] * src.m[15];

	ret.m[12]= m[12] * src.m[0] + m[13] * src.m[4] + m[14] * src.m[8] + m[15] * src.m[12];
	ret.m[13]= m[12] * src.m[1] + m[13] * src.m[5] + m[14] * src.m[9] + m[15] * src.m[13];
	ret.m[14]= m[12] * src.m[2] + m[13] * src.m[6] + m[14] * src.m[10] + m[15] * src.m[14];
	ret.m[15]= m[12] * src.m[3] + m[13] * src.m[7] + m[14] * src.m[11] + m[15] * src.m[15];

	return ret;
}

Matrix4 Matrix4::preMul(const Matrix4 &src) const 
{
	Matrix4 ret;

	ret.m[0] = m[0] * src.m[0] + m[4] * src.m[1] + m[8] * src.m[2] + m[12] * src.m[3];
	ret.m[1] = m[1] * src.m[0] + m[5] * src.m[1] + m[9] * src.m[2] + m[13] * src.m[3];
	ret.m[2] = m[2] * src.m[0] + m[6] * src.m[1] + m[10]* src.m[2] + m[14] * src.m[3];
	ret.m[3] = m[3] * src.m[0] + m[7] * src.m[1] + m[11]* src.m[2] + m[15] * src.m[3];

	ret.m[4] = m[0] * src.m[4] + m[4] * src.m[5] + m[8] * src.m[6] + m[12] * src.m[7];
	ret.m[5] = m[1] * src.m[4] + m[5] * src.m[5] + m[9] * src.m[6] + m[13] * src.m[7];
	ret.m[6] = m[2] * src.m[4] + m[6] * src.m[5] + m[10]* src.m[6] + m[14] * src.m[7];
	ret.m[7] = m[3] * src.m[4] + m[7] * src.m[5] + m[11]* src.m[6] + m[15] * src.m[7];

	ret.m[8] = m[0] * src.m[8] + m[4] * src.m[9] + m[8] * src.m[10] + m[12] * src.m[11];
	ret.m[9] = m[1] * src.m[8] + m[5] * src.m[9] + m[9] * src.m[10] + m[13] * src.m[11];
	ret.m[10]= m[2] * src.m[8] + m[6] * src.m[9] + m[10] * src.m[10] + m[14] * src.m[11];
	ret.m[11]= m[3] * src.m[8] + m[7] * src.m[9] + m[11] * src.m[10] + m[15] * src.m[11];

	ret.m[12]= m[0] * src.m[12] + m[4] * src.m[13] + m[8] * src.m[14] + m[12] * src.m[15];
	ret.m[13]= m[1] * src.m[12] + m[5] * src.m[13] + m[9] * src.m[14] + m[13] * src.m[15];
	ret.m[14]= m[2] * src.m[12] + m[6] * src.m[13] + m[10] * src.m[14] + m[14] * src.m[15];
	ret.m[15]= m[3] * src.m[12] + m[7] * src.m[13] + m[11] * src.m[14] + m[15] * src.m[15];

	return ret;
}

Matrix4 Matrix4::operator*(Matrix4 *src)
{
  Matrix4 ret;

  ret.m[0] = m[0] * src->m[0] + m[4] * src->m[1] + m[8] * src->m[2] + m[12] * src->m[3];
  ret.m[1] = m[1] * src->m[0] + m[5] * src->m[1] + m[9] * src->m[2] + m[13] * src->m[3];
  ret.m[2] = m[2] * src->m[0] + m[6] * src->m[1] + m[10]* src->m[2] + m[14] * src->m[3];
  ret.m[3] = m[3] * src->m[0] + m[7] * src->m[1] + m[11]* src->m[2] + m[15] * src->m[3];

  ret.m[4] = m[0] * src->m[4] + m[4] * src->m[5] + m[8] * src->m[6] + m[12] * src->m[7];
  ret.m[5] = m[1] * src->m[4] + m[5] * src->m[5] + m[9] * src->m[6] + m[13] * src->m[7];
  ret.m[6] = m[2] * src->m[4] + m[6] * src->m[5] + m[10]* src->m[6] + m[14] * src->m[7];
  ret.m[7] = m[3] * src->m[4] + m[7] * src->m[5] + m[11]* src->m[6] + m[15] * src->m[7];

  ret.m[8] = m[0] * src->m[8] + m[4] * src->m[9] + m[8] * src->m[10] + m[12] * src->m[11];
  ret.m[9] = m[1] * src->m[8] + m[5] * src->m[9] + m[9] * src->m[10] + m[13] * src->m[11];
  ret.m[10]= m[2] * src->m[8] + m[6] * src->m[9] + m[10] * src->m[10] + m[14] * src->m[11];
  ret.m[11]= m[3] * src->m[8] + m[7] * src->m[9] + m[11] * src->m[10] + m[15] * src->m[11];

  ret.m[12]= m[0] * src->m[12] + m[4] * src->m[13] + m[8] * src->m[14] + m[12] * src->m[15];
  ret.m[13]= m[1] * src->m[12] + m[5] * src->m[13] + m[9] * src->m[14] + m[13] * src->m[15];
  ret.m[14]= m[2] * src->m[12] + m[6] * src->m[13] + m[10] * src->m[14] + m[14] * src->m[15];
  ret.m[15]= m[3] * src->m[12] + m[7] * src->m[13] + m[11] * src->m[14] + m[15] * src->m[15];


  return ret;
}

void Matrix4::operator*=(const Matrix4 &src)
{
  (*this) = (*this) * src;
}


void Matrix4::rotate(float angle, const Vector3 &vec)
{
  Matrix4 ret;
  vec_t c = (vec_t)cos(angle);
  vec_t s = (vec_t)sin(angle);
  vec_t mc = 1 - c;
  vec_t x = vec.x;
  vec_t y = vec.y;
  vec_t z = vec.z;

  setIdentity();

  ret.m[0] = (x * x * mc) + c;
  ret.m[1] = (y * x * mc) + (z * s);
  ret.m[2] = (x * z * mc) - (y * s);
  ret.m[4] = (x * y * mc) - (z * s);
  ret.m[5] = (y * y * mc) + c;
  ret.m[6] = (y * z * mc) + (x * s);
  ret.m[8] = (x * z * mc) + (y * s);
  ret.m[9] = (y * z * mc) - (x * s);
  ret.m[10]= (z * z * mc) + c;

  (*this) *= ret;
}

void Matrix4::rotateX(float angle)
{
  //setIdentity();
  float c = cos(angle);
  float s = sin(angle);

  m[5] = c;
  m[6] = s;
  m[9] = -s;
  m[10]= c;
}

void Matrix4::rotateY(float angle)
{
  //setIdentity();
  float c = (float)cos(angle);
  float s = (float)sin(angle);

  m[0] = c;
  m[2] = -s;
  m[8] = s;
  m[10]= c;
}

void Matrix4::rotateZ(float angle)
{
  //setIdentity();
  float c = (float)cos(angle);
  float s = (float)sin(angle);

  m[0] = c;
  m[1] = s;
  m[4] = -s;
  m[5] = c;
}

void Matrix4::scale(const Vector3 &S)
{
	m[0]  *= S.x;
	m[5]  *= S.y;
	m[10] *= S.z;
}

void Matrix4::setScale(const Vector3 &S)
{
	m[0]  = S.x;
	m[5]  = S.y;
	m[10] = S.z;
}

void Matrix4::translate(const Vector3 &T)
{
	m[12]+=T.x;
	m[13]+=T.y;
	m[14]+=T.z;
}

void Matrix4::setTranslation(const Vector3 &T)
{
	m[12] = T.x;
	m[13] = T.y;
	m[14] = T.z;
}

void Matrix4::setTranslation(const float &x, const float &y, const float &z) {
	m[12] = x;
	m[13] = y;
	m[14] = z;
}

math::Vector3 Matrix4::getTranslation() const
{
	return math::Vector3(m[12], m[13], m[14]);
}

void Matrix4::transform(Vector3 &C) const
{
  float x,y,z;

  x=C.x;
  y=C.y;
  z=C.z;

  C.x= m[0]*x + m[4]*y + m[8]*z + m[12];
  C.y= m[1]*x + m[5]*y + m[9]*z + m[13];
  C.z= m[2]*x + m[6]*y + m[10]*z + m[14];
}

void Matrix4::setIdentity()
{
  m[0]=1;  m[1]=0;  m[2]=0;  m[3]=0;
  m[4]=0;  m[5]=1;  m[6]=0;  m[7]=0;
  m[8]=0;  m[9]=0;  m[10]=1; m[11]=0;
  m[12]=0; m[13]=0; m[14]=0; m[15]=1;
}

bool Matrix4::inv()
{
  /* NB. OpenGL Matrices are COLUMN major. */
#define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; }
#define MAT(m,r,c) (m)[(c)*4+(r)]

  float wtmp[4][8];
  float m0, m1, m2, m3, s;
  float *r0, *r1, *r2, *r3;

  r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];

  r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1),
  r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3),
  r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,

  r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1),
  r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3),
  r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,

  r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1),
  r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3),
  r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,

  r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1),
  r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3),
  r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;

  /* choose pivot - or die */
  if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2);
  if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1);
  if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0);
  if (0.0 == r0[0]) {
    return false;
  }

  /* eliminate first variable     */
  m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
  s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
  s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
  s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
  s = r0[4];
  if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
  s = r0[5];
  if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
  s = r0[6];
  if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
  s = r0[7];
  if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }

  /* choose pivot - or die */
  if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2);
  if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1);
  if (0.0 == r1[1]) {
    return false;
  }

  /* eliminate second variable */
  m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
  r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
  r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
  s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
  s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
  s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
  s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }

  /* choose pivot - or die */
  if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2);
  if (0.0 == r2[2]) {
    return false;
  }

  /* eliminate third variable */
  m3 = r3[2]/r2[2];
  r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
  r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
  r3[7] -= m3 * r2[7];

  /* last check */
  if (0.0 == r3[3]) {
    return false;
  }

  s = 1.0f/r3[3];              /* now back substitute row 3 */
  r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;

  m2 = r2[3];                 /* now back substitute row 2 */
  s  = 1.0f/r2[2];
  r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
  r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
  m1 = r1[3];
  r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
  r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
  m0 = r0[3];
  r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
  r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;

  m1 = r1[2];                 /* now back substitute row 1 */
  s  = 1.0f/r1[1];
  r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
  r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
  m0 = r0[2];
  r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
  r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;

  m0 = r0[1];                 /* now back substitute row 0 */
  s  = 1.0f/r0[0];
  r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
  r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);

  MAT(m,0,0) = r0[4]; MAT(m,0,1) = r0[5],
  MAT(m,0,2) = r0[6]; MAT(m,0,3) = r0[7],
  MAT(m,1,0) = r1[4]; MAT(m,1,1) = r1[5],
  MAT(m,1,2) = r1[6]; MAT(m,1,3) = r1[7],
  MAT(m,2,0) = r2[4]; MAT(m,2,1) = r2[5],
  MAT(m,2,2) = r2[6]; MAT(m,2,3) = r2[7],
  MAT(m,3,0) = r3[4]; MAT(m,3,1) = r3[5],
  MAT(m,3,2) = r3[6]; MAT(m,3,3) = r3[7];

  return true;

#undef MAT
#undef SWAP_ROWS
}

void Matrix4::print() const
{
  for (int i=0; i<16; ++i)
    std::cout << m[i] << " ";
  std::cout << std::endl;
}

bool Matrix4::cmp(const Matrix4 &A) const
{
  return *this==A;
}

void Matrix4::mul(const Matrix4 &A, const Matrix4 &B)
{
  *this = A*B;
}

void Matrix4::interpolate(const Matrix4 &a, const Matrix4 &b, float t)
{
	Quaternion qlerp;
	Vector3    vlerp;
	// Get the interpolated position
	vlerp = vlerp.lerp(a.getTranslation(), b.getTranslation(), t);
	// Interpolate the rotation
	qlerp.slerp(a.getQuaternion(), b.getQuaternion(), t);
	qlerp.normalize();

	// There is a bug in either Quaternion::getMatrix4() or Matrix4::getQuaternion()!!! Some rotation gets inverted!
	// You can check, since the rotation of A.getQuaternion().getMatrix4() is not the same as A

	// Set the return matrix
	*this = qlerp.getMatrix4();
	//*this = a.getQuaternion().getMatrix4();
	//*this = a;
	setTranslation(vlerp);
}



Quaternion Matrix4::getQuaternion() const
{
	/// Algorithm: Game Programming Gems p.202 ff 
	// Implementation: ODE
	float tr,s;
	Quaternion q;
	tr = _m(0,0) + _m(1,1) + _m(2,2);
	if (tr >= 0) {
		s = sqrtf(tr + 1);
		q.w = 0.5f * s;
		s = 0.5f * (1/s);
		q.x = (_m(2,1) - _m(1,2)) * s;
		q.y = (_m(0,2) - _m(2,0)) * s;
		q.z = (_m(1,0) - _m(0,1)) * s;
		return q;
	}else {
		// find the largest diagonal element and jump to the appropriate case
		// Dirty but the way it works in ODE!
		if (_m(1,1) > _m(0,0)) {
			if (_m(2,2) > _m(1,1)) goto case_2;
			goto case_1;
		}
		if (_m(2,2) > _m(0,0)) goto case_2;
		goto case_0;   
	
		case_0:
		s = sqrtf((_m(0,0) - (_m(1,1) + _m(2,2))) + 1);
		q.x = 0.5f * s;
		s = 0.5f * (1/s);
		q.y = (_m(0,1) + _m(1,0)) * s;
		q.z = (_m(2,0) + _m(0,2)) * s;
		q.w = (_m(2,1) - _m(1,2)) * s;
		return q;

		case_1:
		s = sqrtf((_m(1,1) - (_m(2,2) + _m(0,0))) + 1);
		q.y = 0.5f * s;
		s = 0.5f * (1/s);
		q.z = (_m(1,2) + _m(2,1)) * s;
		q.x = (_m(0,1) + _m(1,0)) * s;
		q.w = (_m(0,2) - _m(2,0)) * s;
		return q;

		case_2:
		s = sqrtf((_m(2,2) - (_m(0,0) + _m(1,1))) + 1);
		q.z = 0.5f * s;
		s = 0.5f * (1/s);
		q.x = (_m(2,0) + _m(0,2)) * s;
		q.y = (_m(1,2) + _m(2,1)) * s;
		q.w = (_m(1,0) - _m(0,1)) * s;
		return q;		
	}
}

Quaternion Matrix4::getQuaternionFLIPPED() const
{
	/// Algorithm: Game Programming Gems p.202 ff 
	// Implementation: ODE
	float tr,s;
	Quaternion q;
	tr = _mf(0,0) + _mf(1,1) + _mf(2,2);
	if (tr >= 0) {
		s = sqrtf(tr + 1);
		q.w = 0.5f * s;
		s = 0.5f * (1/s);
		q.x = (_mf(2,1) - _mf(1,2)) * s;
		q.y = (_mf(0,2) - _mf(2,0)) * s;
		q.z = (_mf(1,0) - _mf(0,1)) * s;
		return q;
	}else {
		// find the largest diagonal element and jump to the appropriate case
		// Dirty but the way it works in ODE!
		if (_mf(1,1) > _mf(0,0)) {
			if (_mf(2,2) > _mf(1,1)) goto case_2;
			goto case_1;
		}
		if (_mf(2,2) > _mf(0,0)) goto case_2;
		goto case_0;   
	
		case_0:
		s = sqrtf((_mf(0,0) - (_mf(1,1) + _mf(2,2))) + 1);
		q.x = 0.5f * s;
		s = 0.5f * (1/s);
		q.y = (_mf(0,1) + _mf(1,0)) * s;
		q.z = (_mf(2,0) + _mf(0,2)) * s;
		q.w = (_mf(2,1) - _mf(1,2)) * s;
		return q;

		case_1:
		s = sqrtf((_mf(1,1) - (_mf(2,2) + _mf(0,0))) + 1);
		q.y = 0.5f * s;
		s = 0.5f * (1/s);
		q.z = (_mf(1,2) + _mf(2,1)) * s;
		q.x = (_mf(0,1) + _mf(1,0)) * s;
		q.w = (_mf(0,2) - _mf(2,0)) * s;
		return q;

		case_2:
		s = sqrtf((_mf(2,2) - (_mf(0,0) + _mf(1,1))) + 1);
		q.z = 0.5f * s;
		s = 0.5f * (1/s);
		q.x = (_mf(2,0) + _mf(0,2)) * s;
		q.y = (_mf(1,2) + _mf(2,1)) * s;
		q.w = (_mf(1,0) - _mf(0,1)) * s;
		return q;
	}
}

Matrix3::Matrix3(const Matrix4 &src)
{
	m[0]=src.m[0];   m[1]=src.m[1];   m[2]=src.m[2];
	m[3]=src.m[4];   m[4]=src.m[5];   m[5]=src.m[6];
	m[6]=src.m[8];   m[7]=src.m[9];   m[8]=src.m[10];
}

Matrix3& Matrix3::operator=(const Matrix4 &src)
{
	m[0]=src.m[0];   m[1]=src.m[1];   m[2]=src.m[2];
	m[3]=src.m[4];   m[4]=src.m[5];   m[5]=src.m[6];
	m[6]=src.m[8];   m[7]=src.m[9];   m[8]=src.m[10];
	return (*this);
}

} // namespace
